package hu.arguscab.ai.game;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.PriorityQueue;
import java.util.Queue;

import hu.arguscab.ai.Graph;
import hu.arguscab.ai.Node;
import hu.arguscab.ai.Path;

/**
 * Az AStar egy gráfon dolgozik, ami akármilyen adattpíust tartalmazhat.<br>
 * Az AStar egy saját node típust használ AStarNode.<br>
 * Ez a node képes bármilyen adattípust tárolni a maga adatai mellett, ezek:
 * <UL>
 * <li> G
 * <li> H
 * <li> F
 * <li> Parent Node.
 * <li> Stored Data
 * </UL>
 * 
 * @author bbodi
 *
 */
public class AStar<N> {

	private List<AStarNode<N>> closedList = new ArrayList<AStarNode<N>>();
	private Queue<AStarNode<N>> openList = new PriorityQueue<AStarNode<N>>();
	private Map<Node<N>, AStarNode<N>> nodeMap = new HashMap<Node<N>, AStarNode<N>>();
	
	@SuppressWarnings("unused")
	private static class AStarNode<N> implements Comparable<AStarNode<N>> {
		private int f, g, h;
		private AStarNode<N> parent;
		private boolean isInClosedList;
		private Node<N> data;
		
		public AStarNode(Node<N> node) {
			this.data = node;
		}
		
		public int getF() 							{return f;}
		public void setF(int f) 					{this.f = f;}
		public int getG() 							{return g;}
		public void setG(int g) 					{this.g = g;}
		public int getH() 							{return h;}
		public void setH(int h) 					{this.h = h;}
		public AStarNode<N> getParent() 			{return parent;}
		public void setParent(AStarNode<N> parent) 	{this.parent = parent;}
		public Node<N> getData() 					{return data;}
		public void setData(Node<N> data) 			{this.data = data;}
		
		public boolean isInClosedList() 			{return isInClosedList;}
		public void setInClosedList(boolean isInClosedList) {this.isInClosedList = isInClosedList;}
		
		@Override
		public int compareTo(AStarNode<N> o) {
			return this.f - o.f;
		}
				
		public <E> void calc(Graph<E, N> graph) {
			assert parent != null;
			int cost = graph.getEdge(parent.data, data).getWeight();
			g = parent.getG() + cost;
			h = 0;
			f = g + h;
		}
		
		public void reCalc(AStarNode<N> parentData) {
			this.parent = parentData;
			g = parentData.getG();
			f = g + h;
		}
		
		public boolean isTarget(Node<N> data) {
			return this.data.equals(data);
		}
		
		@Override
		public String toString() {
			return "[("+data+")G: " + g + ", H: " + h + ", F: " + f+"]";
		}

	}
	
	public <E> Path<N> getPath(Graph<E, N> graph, Node<N> start, Node<N> end) {
		AStarNode<N> node = search(graph, start, end);
		if ( node == null )
			return null;
		Path<N> path = new Path<N>();
		do {
			path.addFirst(node.getData().getData());
		} while ( (node=node.getParent()) != null );
		return path;
	}
	
	private <E> AStarNode<N> search(Graph<E, N> graph, Node<N> start, Node<N> end) {
		// (1)
		addToOpenList(start);
		
		while( true ) {
			// (2) Look for the lowest F cost on the open list and remove them from there.
			AStarNode<N> from = pollOpenList();
			
			// (2.a) Stop when the Open List is empty or the Target node is added to the Closed List.
			if ( from == null )
				return null;						// There is no path
			else if ( from.isTarget(end) )
				return from;						// FOUND
			// (2.b) Put it to the Closed list.
			addToClosedList(from);
			// (2.c) For each adjacent to this current node 
			for ( Node<N> child : graph.adjacentTo( from.getData() ) ) {
				AStarNode<N> to = getFromList(child);
				
				// (2.e) If it isn’t on the open list, add it to the open list. 
				//       Make the current square the parent of this square. 
				//       Record the F, G, and H costs of the square. 
				if ( to == null ) {
					to = new AStarNode<N>(child);
					to.setParent(from);
					to.calc(graph);
					addToOpenList(to);
				} else {
					// (2.d) If it is on the closed list, ignore it.
					if ( to.isInClosedList() == true ) {
						continue;
					}

					// (2.f) If it is on the open list already, check to see if this path to that 
					//       square is better, using G cost as the measure. 
					//       A lower G cost means that this is a better path. 
					//       If so, change the parent of the square to the current square, 
					//       and recalculate the G and F scores of the square. 
					//       If you are keeping your open list sorted by F score, 
					//       you may need to resort the list to account for the change.
					int cost = graph.getEdge(from.getData(), to.getData()).getWeight();
					int newG = from.getG() + cost;
					if ( to.getG() > newG ) {
						to.reCalc(from); 
						reOrder(to); // reorder the open list
					}
				}
			}
		}
	}
	
	/**
	 * @param node The Node from the Graph we perform searching on.
	 * @return an AStarNode if the <i>node</i> was already processed.
	 */
	private AStarNode<N> getFromList(Node<N> node) {
		return nodeMap.get(node);
	}

	private AStarNode<N> addToOpenList(Node<N> node) {
		AStarNode<N> data = new AStarNode<N>(node);
		addToOpenList(data);
		return data;
	}

	
	private void addToOpenList(AStarNode<N> data) {
		openList.add(data);
		nodeMap.put(data.getData(), data);
	}
	
	private void reOrder(AStarNode<N> data) {
		openList.remove(data);
		openList.add(data);
	}
	
	private AStarNode<N> pollOpenList() {
		AStarNode<N> ret = openList.poll();
		return ret;
	}
	
	private void addToClosedList(AStarNode<N> node) {
		node.setInClosedList(true);
		closedList.add(node);
	}
}
